#!/usr/bin/env python
# encoding: utf-8
'''
@author: wangjianrong
@software: pycharm
@file: 卡尔曼滤波.py
@time: 2020/9/28 15:29
@desc:
https://zhuanlan.zhihu.com/p/77327349
这个实践的已知量是导弹做水平运动，已知导弹每个时刻的速度dv,和速度测量仪器的标准差是v_std，
还已知每个时刻用GPS测量出导弹位置position_noise以及GPS的方差是predict_var。（注意标准差的平方是方差，不用觉得奇怪）
我们需要用卡尔曼滤波根据这些信息获得融合两种传感器后的位置信息position_predict
'''

import numpy as np

# 模拟数据
t = np.linspace(1, 100, 100)
a = 0.5
position = (a * t ** 2) / 2

position_noise = position + np.random.normal(0, 120, size=(t.shape[0]))   #相当于Zt

import matplotlib.pyplot as plt

plt.plot(t, position, label='truth position')
plt.plot(t, position_noise, label='only use measured position')

# 初试的估计导弹的位置就直接用GPS测量的位置
predicts = [position_noise[0]]
position_predict = predicts[0]

predict_var = 0  #Xt-1的方差，这个方差是累积的
odo_var = 120 ** 2  # 这是我们自己设定的位置测量仪器的方差，越大则测量值占比越低  雷达的方差
v_std = 50  # 测量仪器的方差  控制设备的标准差
for i in range(1, t.shape[0]):
    dv = (position[i] - position[i - 1]) + np.random.normal(0, 50)  # 模拟从IMU读取出的速度  position[i] - position[i - 1]是理论上的速度
    position_predict = position_predict + dv  # 利用上个时刻的位置和速度预测当前位置  position_predict相当于Xt
    predict_var += v_std ** 2  # 更新预测数据的方差，累计控制设备的速度方差
    # 下面是Kalman滤波 Xt的分布是N(Xt,predict_var) Zt的分布是N(position_noise[i],odo_var)
    # 卡尔曼增益就是 predict_var/(odo_var+predict_var),Zt的权重
    position_predict = position_predict * odo_var / (predict_var + odo_var) + position_noise[i] * predict_var / (
                predict_var + odo_var)
    predict_var = (predict_var * odo_var) / (predict_var + odo_var) ** 2  #累计Zt和Xt的方差
    predicts.append(position_predict)

plt.plot(t, predicts, label='kalman filtered position')

plt.legend()
plt.show()